package cn.com.fhc.ros

import cn.com.fhc.ros.messages.Message
import javax.json.Json

class Topic {

    private var ros: Ros
    private var name: String
    private var type: String
    private var isAdvertised = false
    private var isSubscribed = false
    private var compression: CompressionType = CompressionType.none
    private var throttleRate = 0

    // used to keep track of this object's callbacks
    private var callbacks=  mutableListOf<TopicCallback>()

    // used to keep track of the subscription IDs
    private var ids =  mutableListOf<String>()


    /**
     * Create a ROS topic with the given information. No compression or
     * throttling is used.
     *
     * @param ros
     * A handle to the ROS connection.
     * @param name
     * The name of the topic (e.g., "/cmd_vel").
     * @param type
     * The message type (e.g., "std_msgs/String").
     */
    constructor(ros: Ros, name: String, type: String)  :this(ros, name,type, CompressionType.none, 0)

    /**
     * Create a ROS topic with the given information. No throttling is used.
     *
     * @param ros
     * A handle to the ROS connection.
     * @param name
     * The name of the topic (e.g., "/cmd_vel").
     * @param type
     * The message type (e.g., "std_msgs/String").
     * @param compression
     * The type of compression used for this topic.
     */
    constructor(ros: Ros, name: String, type: String, compression: CompressionType) :this(ros, name,type, compression, 0)

    /**
     * Create a ROS topic with the given information. No compression is used.
     *
     * @param ros
     * A handle to the ROS connection.
     * @param name
     * The name of the topic (e.g., "/cmd_vel").
     * @param type
     * The message type (e.g., "std_msgs/String").
     * @param throttleRate
     * The throttle rate to use for this topic.
     */
    constructor(ros: Ros, name: String, type: String, throttleRate: Int) :this(ros, name,type,
        CompressionType.none, throttleRate)

    /**
     * Create a ROS topic with the given information.
     *
     * @param ros
     * A handle to the ROS connection.
     * @param name
     * The name of the topic (e.g., "/cmd_vel").
     * @param type
     * The message type (e.g., "std_msgs/String").
     * @param compression
     * The type of compression used for this topic.
     * @param throttleRate
     * The throttle rate to use for this topic.
     */
    constructor(
        ros: Ros, name: String, type: String,
        compression: CompressionType, throttleRate: Int
    ) {
        this.ros = ros
        this.name = name
        this.type = type
        this.isAdvertised = false
        this.isSubscribed = false
        this.compression = compression
        this.throttleRate = throttleRate
    }





    /**
     * Subscribe to this topic. A callback function is required and will be
     * called with any incoming message for this topic.
     *
     * @param cb
     * The callback that will be called when incoming messages are
     * received.
     */
    fun subscribe(cb: TopicCallback) {
        // register the callback function
        ros.registerTopicCallback(name, cb)
        // internal reference used during unsubscribe
        callbacks.add(cb)
        val subscribeId = "subscribe:" + name + ":" + ros.nextId()
        ids.add(subscribeId)

        // build and send the rosbridge call
        val call = Json.createObjectBuilder()
            .add(FIELD_OP, OP_CODE_SUBSCRIBE)
            .add(FIELD_ID, subscribeId)
            .add(FIELD_TYPE, type)
            .add(FIELD_TOPIC, name)
            .add(FIELD_COMPRESSION, compression.toString())
            .add(FIELD_THROTTLE_RATE, throttleRate).build()
        ros.send(call)

        // set the flag indicating we have subscribed
        isSubscribed = true
    }

    /**
     * Unregisters as a subscriber for the topic. Unsubscribing will remove all
     * the associated subscribe callbacks.
     */
    fun unsubscribe() {
        // remove this object's associated callbacks.
        for (cb in callbacks) {
            ros.deregisterTopicCallback(name, cb)
        }
        callbacks.clear()

        // build and send the rosbridge calls
        for (id in ids) {
            val call = Json.createObjectBuilder()
                .add(FIELD_OP, OP_CODE_UNSUBSCRIBE)
                .add(FIELD_ID, id)
                .add(FIELD_TOPIC, name).build()
            ros.send(call)
        }

        // set the flag indicating we are not longer subscribed
        isSubscribed = false
    }

    /**
     * Registers as a publisher for the topic. This call will be automatically
     * called by publish if you do not explicitly call it.
     */
    fun advertise() {
        // build and send the rosbridge call
        val advertiseId = "advertise:" + name + ":" + ros.nextId()
        val call = Json.createObjectBuilder()
            .add(FIELD_OP, OP_CODE_ADVERTISE)
            .add(FIELD_ID, advertiseId)
            .add(FIELD_TYPE, type)
            .add(FIELD_TOPIC, name).build()
        ros.send(call)

        // set the flag indicating we are registered
        isAdvertised = true
    }

    /**
     * Unregister as a publisher for the topic.
     */
    fun unadvertise() {
        // build and send the rosbridge call
        val unadvertiseId = ("unadvertise:" + name + ":"
                + ros.nextId())
        val call = Json.createObjectBuilder()
            .add(FIELD_OP, OP_CODE_UNADVERTISE)
            .add(FIELD_ID, unadvertiseId)
            .add(FIELD_TOPIC, name).build()
        ros.send(call)

        // set the flag indicating we are no longer registered
        isAdvertised = false
    }

    /**
     * Publish the given message to ROS on this topic. If the topic is not
     * advertised, it will be advertised first.
     *
     * @param message
     * The message to publish.
     */
    fun publish(message: Message) {
        // check if we have advertised yet.
        if (!isAdvertised) {
            advertise()
        }

        // build and send the rosbridge call
        val publishId = "publish:" + name + ":" + ros.nextId()
        val call = Json.createObjectBuilder()
            .add(FIELD_OP, OP_CODE_PUBLISH)
            .add(FIELD_ID, publishId)
            .add(FIELD_TOPIC, name)
            .add(FIELD_MESSAGE, message.toJsonObject()).build()
        ros.send(call)
    }
}